/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#!/usr/bin/python
# Example of Map server in python

import sys, threading, math, time, random, array
sys.path.append("../../../../GIMI/wrap/Python/lib/")
sys.path.append("../../../interfaces/wrap/Python/lib/")
import MapServer as MS
import gimi
import occudb


def parseArgs(args):
	parsedArgs = {}
	nextIsValueOf = ""
	for arg in args:
		if not arg:
			continue
		if arg[0] == '-':
			nextIsValueOf = arg[1:]
			parsedArgs[nextIsValueOf] = ""
		else:
			if nextIsValueOf != "":
				parsedArgs[nextIsValueOf] = arg
				nextIsValueOf = ""
	return parsedArgs



def printUsage():
	print """
This is an example of how to use wrapped Map Server with Python.
 
Usage is:	%s [OPTIONS]
  [-u addr]			Use 'addr' as GIMnet AccessPoints address
  [-p port]			Use 'port' as GIMnet AccessPoints port
  [-n name]			Use 'name' as this nodes GIMnet name
  [-G name]			Specify the MaCIGroup for this module
  [-I name]			Specify the MaCIGroup Interface Instance

  
  [-v]				Increase verbosity by 1
  [-q]				Quiet mode
  [-h]				print this
	"""%sys.argv[0]

		
name = "testPyMapServer22"
groupName = "hinkuvinku22"
instanceName = "suurvisiiri22"
server = "asrobo.hut.fi"
port = 40002

args = parseArgs(sys.argv)
print args

if args.has_key("h"):
	printUsage()
	sys.exit()

if args.has_key("u"):server = args['u']
if args.has_key("p"):port = int(args['p'])
if args.has_key("n"):name = args['n']
if args.has_key("G"):groupName = args['G']
if args.has_key("I"):instanceName = args['I']



#MaCI connection
g = gimi.GIMI()
mcs = MS.CMaCICtrlServer(g)
ms = MS.CMapServer(mcs, 0)

mcs.SetGroupName(groupName) 
ms.SetInterfaceInstanceName(instanceName)


print "Connecting to %s:%s as %s..."%(server, port, name), 
ret = g.connectToHubEx(server, port, name)
if ret != MS.KMaCIOK:
	print "Failed: ", MS.GetErrorStr(ret)
	sys.exit()
else: print MS.GetErrorStr(ret)



print "Opening... ",
ret = ms.Open()
if ret != MS.KMaCIOK:
	print "Failed: ", MS.GetErrorStr(ret)
	sys.exit()
else: print MS.GetErrorStr(ret)


del ret
#time.sleep(1.0) # got to sleep to wait for something to get ready to serve us


print "prepare serving"
#Create data to be served 
d = 1.1
reso=0.5 #grid map resolution
number=0 #block numbering
mapData = MS.CMapData()
mapData.CreateInternalBinBag()
MapHeader = MS.TMapHeader()
UpdateHeader = MS.TMapUpdateHeader()
occuBlock = MS.TGridBlockHeader()
occuData = []

#start fetching stuff from occudb
occusource = occudb.DataBase()

if occusource.running:
        print "we have connection to occuDB"
else:
        print "we have occuDB connection PROBLEM"

#get metaID, gridMetaInfo, and Grids from DB at once
metaID = occusource.getMetaID(str('hervanta'))
gridMetaInfo = occusource.getMetaInfo(int(metaID))
grids =  occusource.getGlobalOccuGridCells(int(metaID))

reso = gridMetaInfo[2]
MapHeader.gridDataResolution=float(reso)

mapData.Reset()
mapData.SetMapHeader(MapHeader)

#set block's number
#occupancy grid and height map are considered separate layers so same numbers can be used
occuBlock.number=number

#increment block number
number +=1;

#randomize block location and dimensions
#TODO: these values must be multiples of the resolution!!
#occuBlock.x=random.random()*50-25
#occuBlock.y=random.random()*50-25
occuBlock.x=float(gridMetaInfo[4])
occuBlock.y=float(gridMetaInfo[5])
occuBlock.samples_x=gridMetaInfo[0]
occuBlock.samples_y=gridMetaInfo[1]

occuBlock.dataOriginX=0
occuBlock.dataOriginY=0
    
occuBlock.dataType=MS.KGridTypeOccupancy
for grid in grids:
	occuData.append(int(grid['occu']*255))
#for i in range(occuBlock.samples_x*occuBlock.samples_y): occuData.append(int(random.random()*100+100))
# add few more just in case
#occuData.append(int(random.random()*100+100))
#occuData.append(int(random.random()*100+100))
occuBlock.dataSize=occuBlock.samples_x*occuBlock.samples_y*MS.KGridTypeOccupancy #data size in bytes

#draw line to data for visualization chekup

#for i in range(occuBlock.samples_x/2*occuBlock.samples_y + occuBlock.samples_y/4, 
#	occuBlock.samples_x/2*occuBlock.samples_y + 3*occuBlock.samples_y/4):
#	occuData[i] = 1

aa = array.array('B', occuData)
print aa
print aa.tostring().__str__()
if not mapData.AddGridBlock(occuBlock, aa.tostring()):
	print "Failed to add occupancy grid block to whole map's data"

bb =  mapData.GetGridBlockHeader(0)
if bb:
	print "sending occu block: x=%f y=%f w=%d h=%d ox=%d oy=%d"%(bb.x, bb.y,
	bb.samples_x, bb.samples_y, bb.dataOriginX, bb.dataOriginY)
else:
	print bb.__str__()

ms.SetMap(mapData)



print "Serve hungry clients",
try:
	while True:

		time.sleep(d)
		print ".",
		sys.stdout.flush()
#    if ms.SendMapUpdateEvent(MapUpdateData):
#			print "Sent map update"
			#MapUpdateData.Print(2);
    
#    MapUpdateData.Reset()
    
#    occuData = []

except KeyboardInterrupt:
	print "\nClosing %s"%name
ms.Close()


